/*=========================================================================
UberSim Source Code Release
-------------------------------------------------------------------------
Copyright (C) 2002 Manuela Veloso, Brett Browning, Mike Bowling,
                   James Bruce; {mmv, brettb, mhb, jbruce}@cs.cmu.edu
                   Erick Tryzelaar {erickt}@andrew.cmu.edu
School of Computer Science, Carnegie Mellon University
-------------------------------------------------------------------------
This software is distributed under the GNU General Public License,
version 2.  If you do not have a copy of this licence, visit
www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
in the hope that it will be useful, but WITHOUT ANY WARRANTY,
including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
-------------------------------------------------------------------------*/

#include <string>

#include "mmgr.h"

#include "Matrix33.h"

/*******************************/

const Matrix33 Matrix33::ZERO     = Matrix33 (0, 0, 0, 0, 0, 0, 0, 0, 0);
const Matrix33 Matrix33::IDENTITY = Matrix33 (1, 0, 0, 0, 1, 0, 0, 0, 1);

/*******************************/

Matrix33 Matrix33::rotateAboutX (Real a)
{
	return Matrix33 (1.0,           0.0,            0.0, 
	                 0.0, Math::cos (a), -Math::sin (a), 
	                 0.0, Math::sin (a),  Math::cos (a)); 
}

/*******************************/

Matrix33 Matrix33::rotateAboutY (Real a)
{
	return Matrix33 ( Math::cos (a), 0.0, Math::sin (a), 
	                            0.0, 1.0,           0.0, 
	                 -Math::sin (a), 0.0, Math::cos (a));
}

/*******************************/

Matrix33 Matrix33::rotateAboutZ (Real a)
{
	return Matrix33 (Math::cos (a), -Math::sin (a), 0.0, 
	                 Math::sin (a),  Math::cos (a), 0.0, 
	                           0.0,            0.0, 1.0);
}

/*******************************/

Matrix33 Matrix33::rotate (Real a, const Vector3& axis)
{
	//axis.normalize ()
	
	Matrix33 s = Matrix33 (    0.0, -axis.z,  axis.y, 
	                        axis.z,     0.0, -axis.x,
	                       -axis.y,  axis.x,     0.0);

	return Matrix33::IDENTITY + Math::sin (a) * s + (1 - Math::cos (a)) * s * s;
}

/*******************************/

Matrix33::Matrix33 ()
{
}

/*******************************/

Matrix33::Matrix33 (const Matrix33& matrix)
{
	memcpy (m, matrix.m, 9 * sizeof (Real));
}

/*******************************/

Matrix33::Matrix33 (const Real matrix[3][3])
{
	memcpy (m, matrix, 9 * sizeof (Real));
}

/*******************************/

Matrix33::Matrix33 (const Real m00, const Real m01, const Real m02, 
                    const Real m10, const Real m11, const Real m12, 
                    const Real m20, const Real m21, const Real m22)
{
	m[0][0] = m00;
	m[0][1] = m01;
	m[0][2] = m02;
	m[1][0] = m10;
	m[1][1] = m11;
	m[1][2] = m12;
	m[2][0] = m20;
	m[2][1] = m21;
	m[2][2] = m22;
}

/*******************************/

Matrix33::Matrix33 (const Vector3& euler)
{
	Matrix33 rx = Matrix33::rotateAboutX (euler.x);
	Matrix33 ry = Matrix33::rotateAboutY (euler.y);
	Matrix33 rz = Matrix33::rotateAboutZ (euler.z);

	Matrix33 m = rx * ry * rz;

	*this = m;
}

/*******************************/

Matrix33::~Matrix33 ()
{
}

/*******************************/

Real* Matrix33::operator [] (const unsigned int r) const
{
  return (Real*)&m[r][0];
}

/*******************************/
/*
Matrix33::operator Real* ()
{
	return &m[0][0];
}
*/
/*******************************/

Matrix33& Matrix33::operator = (const Matrix33& matrix)
{
	memcpy (m, matrix.m, 9 * sizeof (Real));
	
	return *this;
}

/*******************************/

Matrix33& Matrix33::operator = (const Real* matrix)
{
 	m[0][0] = matrix[0];
	m[0][1] = matrix[1];
	m[0][2] = matrix[2];
	m[1][0] = matrix[3];
	m[1][1] = matrix[4];
	m[1][2] = matrix[5];
	m[2][0] = matrix[6];
	m[2][1] = matrix[7];
	m[2][2] = matrix[8];

	return *this;
}

/*******************************/

Matrix33 Matrix33::operator + (const Matrix33& matrix) const
{
	Matrix33 out;
	
	for (unsigned int r = 0; r < 3; r++)
	{
		for (unsigned int c = 0; c < 3; c++)
		{
			out.m[r][c] = m[r][c] + matrix.m[r][c];
		}
	}

	return out;
}

/*******************************/

Matrix33 Matrix33::operator - (const Matrix33& matrix) const
{
	Matrix33 out;

	for (unsigned int r = 0; r < 3; r++)
	{
		for (unsigned int c = 0; c < 3; c++)
		{
			out.m[r][c] = m[r][c] - matrix.m[r][c];
		}
	}

	return out;
}

/*******************************/

Matrix33 Matrix33::operator * (const Real scalar) const
{
	Matrix33 out;

	for (unsigned int r = 0; r < 3; r++)
	{
		for (unsigned int c = 0; c < 3; c++)
		{
			out.m[r][c] = scalar * m[r][c];
		}
	}

	return out;
}

/*******************************/

Matrix33 operator * (const Real scalar, const Matrix33& matrix)
{
	Matrix33 out;

	for (unsigned int r = 0; r < 3; r++)
	{
		for (unsigned int c = 0; c < 3; c++)
		{
			out.m[r][c] = scalar * matrix.m[r][c];
		}
	}

	return out;
}

/*******************************/

Vector3 Matrix33::operator * (const Vector3& vector) const
{
	Vector3 out;

	for (unsigned int r = 0; r < 3; r++)
	{
		out[r] = m[r][0]*vector[0] + m[r][1]*vector[1] + m[r][2]*vector[2];
	}

	return out;
}

/*******************************/

Vector3 operator * (const Vector3& vector, const Matrix33& matrix)
{
	Vector3 out;

	for (unsigned int r = 0; r < 3; r++)
	{
		out[r] = vector[0]*matrix.m[0][r] + vector[1]*matrix.m[1][r] + vector[2]*matrix.m[2][r];
	}

	return out;
}

/*******************************/

Matrix33 Matrix33::operator * (const Matrix33& matrix) const
{
	Matrix33 out;

	for (unsigned int r = 0; r < 3; r++)
	{
		for (unsigned int c = 0; c < 3; c++)
		{
			out.m[r][c] = m[r][0]*matrix.m[0][c] + m[r][1]*matrix.m[1][c] + m[r][2]*matrix.m[2][c];
		}
	}

	return out;
}

/*******************************/

Matrix33 Matrix33::transpose ()
{
	return Matrix33 (m[0][0], m[1][0], m[2][0], 
	                 m[0][1], m[1][1], m[2][1], 
	                 m[0][2], m[1][2], m[2][2]);
}

/*******************************/

Vector3 Matrix33::eulerAngles ()
{
	Vector3 euler;

	euler.y = Math::asin (m[0][2]);

	if (euler.y < Math::HALF_PI)
	{
		if (euler.y > -Math::HALF_PI)
		{
			euler.x = Math::atan2 (-m[1][2], m[2][2]);
			euler.z = Math::atan2 (-m[0][1], m[0][0]);
		}
		else
		{
			euler.x = -Math::atan2 (m[1][0], m[1][1]);
			euler.z = 0;
		}
	}
	else
	{
		euler.x = Math::atan2 (m[1][0], m[1][1]);
		euler.z = 0;
	}

	return euler;
}

/*******************************/
